package testtiles;

import butines.core.math2d.Vector2D;
import butines.core.math2d.Vector2DUtils;
import butines.game.steering.SteerLibrary;
import butines.game.steering.Transformation;
import butines.game.steering.Vehicle;

public abstract class BasicVehicle implements Vehicle {

	private Vector2D forward = new Vector2D(0, 1);
	private Vector2D side = new Vector2D(-1, 0);
	private Vector2D accel = new Vector2D(0, 0);

	private Vector2D previousPosition = new Vector2D();
	private Vector2D predictFuturePosition = new Vector2D();
	
	private float speed;
	private float mass = 1;
	private float maxForce = 3;
	
	public Vector2D getPredictFuturePosition() {
    	predictFuturePosition.set(getPosition());
    	predictFuturePosition.add(getVelocity());
		return predictFuturePosition;
	}
	
	public Vector2D getPreviousPosition() {
		return previousPosition;
	}

	// implements LocalSpace //
	private Transformation trans;

	private Transformation trans() {
		if (trans == null) {
			trans = new Transformation();
		}
		trans.setForward(forward);
		trans.setPosition(getPosition());
		trans.setSide(side);
		return trans;
	}


	public Vector2D getSide() {
		return side;
	}

	public Vector2D getForward() {
		return forward;
	}

	public Vector2D toLocalPoint(Vector2D globalPoint) {
		return trans().toLocalPoint(globalPoint);
	}

	public Vector2D toLocalVector(Vector2D globalVector) {
		return trans().toLocalVector(globalVector);
	}

	public Vector2D toGlobalVector(Vector2D localVector) {
		return trans().toGlobalVector(localVector);
	}

	public Vector2D toGlobalPoint(Vector2D localPoint) {
		return trans().toGlobalPoint(localPoint);
	}

	// implements Obstacles //
	public Intersection intersectionWith(Vehicle vehicle) {
		return SteerLibrary.intersectionWith(vehicle, this, getPosition(), getRadius());
	}

	public Vector2D steerToAvoid(Vehicle vehicle) {
		return SteerLibrary.steerToAvoid(vehicle, getPosition(), getRadius());
	}
	
	public boolean collidedWith(Vehicle vehicle) {
		if (vehicle == this || vehicle == null) {
			return false;
		}
		float minDistance = vehicle.getRadius() + getRadius();
		return SteerLibrary.collided(getPosition(), vehicle.getPosition(), minDistance);
	}
	
	// implements Vehicle
	private Vector2D[] smoothedHeading;
	private int nextSmoothHeading;

	private void smoothing() {
		int len = smoothedHeading == null ? 12 : smoothedHeading.length;
		if (smoothedHeading == null) {
			smoothedHeading = new Vector2D[len];
			for (int i = 0; i < len; i++) {
				smoothedHeading[i] = new Vector2D();
			}
		}
		
		smoothedHeading[nextSmoothHeading].set(getForward());
		nextSmoothHeading = (nextSmoothHeading < smoothedHeading.length - 1) ? nextSmoothHeading + 1 : 0;

		getHeading().set(0, 0);
		for (int i = 0; i < len; i++) {
			getHeading().add(smoothedHeading[i]);
		}
		getHeading().div(len);
	}
	
	public void applySteeringForce(Vector2D steeringForce) {
		previousPosition.set(getPosition());
		
		accel.set(steeringForce);
		accel.div(getMass());
		Vector2DUtils.truncate(accel, getMaxForce());
		
		getVelocity().add(accel);
		Vector2DUtils.truncate(getVelocity(), getMaxSpeed());
		
		getPosition().add(getVelocity());

		speed = getVelocity().length();
		
		if (speed > 0) {
			getForward().normalise(getVelocity());
			getSide().set(-getForward().y, getForward().x);
		}
		
		smoothing();
		
	}
		
	public void applyBrakingForce(float rate) {
		getVelocity().mult(rate);
		speed = getVelocity().length();
	}
	
	public float getMass() {
		return mass;
	}

	public void setMass(float mass) {
		this.mass = mass;
	}

	public float getMaxForce() {
		return maxForce;
	}

	public void setMaxForce(float maxForce) {
		this.maxForce = maxForce;
	}

	public Vector2D getAccel() {
		return accel;
	}
	
	public float getSpeed() {
		return speed;
	}
	
	public abstract Vector2D getPosition();
	
	public abstract float getMaxSpeed();

	public abstract Vector2D getVelocity();
	
	public abstract Vector2D getHeading();

	public abstract float getRadius();


}
